import rclpy  
from rclpy.node import Node  
from nav_msgs.msg import Odometry   
from geometry_msgs.msg import Twist  
from std_msgs.msg import Float32MultiArray
from sensor_msgs.msg import Imu  
import numpy as np  
from std_msgs.msg import Int32MultiArray
from std_msgs.msg import Int32
from rcl_interfaces.msg import SetParametersResult
from ai_msgs.msg import PerceptionTargets
from origincar_msg.msg import Sign  # 根据图片的消息类型
from origincar_msg.msg import Data
from std_msgs.msg import String
import math
import datetime
import time
from pyzbar.pyzbar import decode

from sensor_msgs.msg import Image
import cv2

from threading import Thread
import keyboard

import sys
import tty
import termios

import os
from openai import OpenAI
import base64
import json
from navigation import FCOS

class InertialNavigationNode(Node):  
    def __init__(self):  
        super().__init__('inertial_navigation_node')  # 初始化节点名称

        self.last_log_time_vel_callback = self.get_clock().now()  # 记录上次日志时间
        self.last_log_time_navagation_0 = self.get_clock().now()
        self.last_log_time_navagation_3 = self.get_clock().now()
        self.task3_info = 0
        self.qrcode_info = 0
        self.model_path = '/root/fcos_test/model/fcos_test4.0.bin'
        # 用于判断是否允许开始导航，1为开始，0为停止
        self.flag = 1

        #用于设置状态，0为简单导航，1为避障导航，2为二维码导航，3为停车点导航  
        #避障导航也就是添加了简单的避障，2和3是将目标点给替代掉  
        self.state = 0

        #目标点切换（任务一任务三启动标志）
        self.task = 0

        #任务中途切换目标位置
        #self.change = 0

        #0为默认，1为启动大模型调用
        self.vlm_flag = 0
        #0为默认，1为启动cv检测
        self.opencv_flag = 0


        # 创建IMU数据订阅
        self.subscription_imu_data = self.create_subscription(  
            Imu,  
            '/imu/data_raw',  
            self.imu_callback,  
            3
        )
        # 创建目标位置订阅，主要用于订阅目标位置，这个的y的位置偏差较大
        self.subscription_robotvel = self.create_subscription(
            Data,  
            '/robotvel',
            self.robotvel_callback,
            3
        )
        # 创建一个订阅者，订阅 /sigh4return 话题，消息类型为 Int32,上位机信号
        self.sign4return = self.create_subscription(
            Int32,
            '/sign4return',
            self.sign4return_callback,
            10)     
        # 创建订阅者，订阅话题 /image
        self.subscription = self.create_subscription(
            Image,
            '/image',
            self.listener_callback,
            3)
        self.subscription_vlm = self.create_subscription(  
            Int32,  
            '/vlm',  
            self.vlm_callback,  
            3)

        # 创建速度命令发布者
        self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)  
        # 创建发布者，发布到 /sign_switch 话题
        self.sign_switch_pub = self.create_publisher(Sign, 'sign_switch', 10)      # 发布二维码的信息
        #创建新的发布者，用于发布避障信息
        self.cone_publisher = self.create_publisher(Int32MultiArray, '/cone', 10)  # 创建新的发布者，用于发布x和y坐标
        #创建新的发布者，用于发布park信息
        self.park_publisher = self.create_publisher(Int32MultiArray, '/park', 10)  # 创建新的发布者，用于发布x和y坐标   
        #创建发布者，发布vlm识别结果
        self.vlm_msg_publisher = self.create_publisher(String, '/vlm_msg', 10)
        #创建发布者，发布二维码识别结果
        self.qrcode_publisher = self.create_publisher(String, '/qrcode', 10)

        #图像识别参数
        self.up = 120
        self.left = 50
        self.area = 1200    
        self.right_park = 640  #580
        self.up_park = 270    
        self.left_cone = 0
        self.right_cone = 0

        # 初始化状态变量
        self.angular =  0
        self.velocity = 0 #阿克曼小车的速度只有x方向
        self.position = np.array([0.0, 0.0])  # 当前位置信息

        self.target_position = np.array([0.0, 0.0])  # 目标位置        
        self.target_angular = 0.0  # 目标角度  
        
        #由于识别到停车场和二维码后会前往，所以这里设置了相对位置
        self.relativ_target_position = np.array([0.0, 0.0])  # 相对目标位置
        self.park_relativ_position = np.array([0.0, 0.0,0.0,0.0])
        
        #锥桶的相对坐标在这里，分别是xy,宽度和高度
        self.zhuitong_relativ_position = np.array([0.0, 0.0,0.0,0.0])
        self.safety_zhuitong_height = 120
        self.safety_zhuitong_width = 140
        self.zhuitong_position = np.array([0.0, 0.0])

        #这里使用了加速度计校准
        self.sample_size =100
        self.angular_velocity_sample = []
        self.bias_correction = 0
        self.imu_z = 0
        self.robotvel_last_time = self.imu_last_time = self.get_clock().now()   
        self.task1_time = 0
        self.task3_time = 0
    
        #参数设置
        self.declare_parameter ('kp',0.04)
        self.declare_parameter ('stop_distance',0.1) 
        self.declare_parameter ('vx',0.5)
        self.declare_parameter ('vx_3',0.25)
        self.declare_parameter ('safe_distance',0.8) 
        self.declare_parameter ('turn_time_1',1.0)
        self.declare_parameter ('turn_time_2',1.2)
        self.declare_parameter ('turn_v',0.4)
        self.declare_parameter ('turn_w',0.01)
        self.declare_parameter ('turn_error',0)
        self.declare_parameter ('zhuitongyuzhi',300) 
        self.declare_parameter ('radius_cone',0.3)

        self.kp = self.get_parameter("kp").value
        self.stop_distance = self.get_parameter("stop_distance").value
        self.vx = self.get_parameter("vx").value
        self.vx_3 = self.get_parameter("vx_3").value
        self.safe_distance = self.get_parameter("safe_distance").value
        self.turn_time_1 = self.get_parameter("turn_time_1").value
        self.turn_time_2 = self.get_parameter("turn_time_2").value
        self.turn_v = self.get_parameter("turn_v").value
        self.turn_w = self.get_parameter("turn_w").value   
        self.turn_error = self.get_parameter("turn_error").value  
        self.zhuitongyuzhi = self.get_parameter("zhuitongyuzhi").value
        self.radius_cone = self.get_parameter("radius_cone").value

        #参数回调函数
        self.add_on_set_parameters_callback(self.parameters_callback)

        # 透视变换矩阵
        '''
        self.matrix = np.array([
            [-4.78185634e-02, -4.35936013e-01, 1.17525583e+02],
            [5.34996953e-03, -8.68712398e-01, 2.17611259e+02],
            [2.24575710e-05, -4.29432464e-03, 1.00000000e+00]
        ])
        '''
    def vlm_callback(self, msg):  #接受按键信息，并设置vlm_flag为1，启动大模型，可设置cv检测
        self.vlm = msg.data
        if self.vlm == 1:
            self.vlm_flag = 1
        if self.vlm == 2:
            self.opencv_flag = 1
        if self.vlm == 3:
            self.opencv_flag = 0

    def qwen_vl(self,frame):    #调用qwen API

        client = OpenAI(
            # 若没有配置环境变量，请用百炼API Key将下行替换为：api_key="sk-xxx",
            api_key=os.getenv("DASHSCOPE_API_KEY"),
            base_url="https://dashscope.aliyuncs.com/compatible-mode/v1",
        )
        #  Base64 编码格式
        # def encode_image(image_path):
        #     with open(image_path, "rb") as image_file:
        #         return base64.b64encode(image_file.read()).decode("utf-8")

        """将 OpenCV 读取的帧转换为 Base64"""
        _, buffer = cv2.imencode(".png", frame)  # 将 frame 转换为 PNG 格式的字节流
        base64_image = base64.b64encode(buffer).decode("utf-8")
            
        completion = client.chat.completions.create(
            model="qwen-vl-plus",  # 此处以qwen-vl-plus为例，可按需更换模型名称。模型列表：https://help.aliyun.com/zh/model-studio/getting-started/models
            messages=[{"role": "user","content": [
                    {"type": "text","text": "中文描述"},
                    {"type": "image_url",
                    #"image_url": {"url": "https://dashscope.oss-cn-beijing.aliyuncs.com/images/dog_and_girl.jpeg"}}
                    "image_url": {"url": f"data:image/png;base64,{base64_image}"}}
                    ]}],
            max_tokens=30,
            )
        response_str = completion.model_dump_json()
        response_dict = json.loads(response_str)
        content = response_dict["choices"][0]["message"]["content"]

        return content

    def listener_callback(self, msg):  #image话题的回调函数，使用cv获取物体参数，设置小车模式；接受vlm_flag开启大模型图生文

        #将图像数据转换为OpenCV格式
        np_arr = np.frombuffer(msg.data, np.uint8)
        cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)

        if(self.vlm_flag == 1):
            vlm_msg = String()
            vlm_msg.data = self.qwen_vl(frame=cv_image)
            self.vlm_msg_publisher.publish(vlm_msg)
            self.get_logger().info(f"大模型识别结果为: {vlm_msg.data}")
            self.vlm_flag = 0

        if(self.opencv_flag == 1):
            #获取锥桶的底边中点与宽高的坐标（x,y,w,h)
            zhuitong_position = self.process_frame(cv_image)
            #把坐标发布出去方便调试
            cone_msg = Int32MultiArray()
            cone_msg.data = zhuitong_position
            self.cone_publisher.publish(cone_msg)

            #获取P点的中点与宽高(x,y,w,h)
            park_position = self.park_frame(cv_image)
            #把坐标发布出去方便调试
            park_msg = Int32MultiArray()
            park_msg.data =  park_position
            self.park_publisher.publish(park_msg)


        if(self.task == 1): 
            time = self.get_clock().now()
            dt_1 = (time - self.task1_time).nanoseconds * 1e-9

            prediction_bbox,class_name = FCOS.fcos(self.model_path,cv_image)
            self.get_logger().info(f"开始FCOS")
            #获取锥桶的底边中点与宽高的坐标（x,y,w,h)
            if class_name == 'obstcle':
                self.zhuitong_relativ_position = prediction_bbox[:4]
                #把坐标发布出去方便调试
                cone_msg = Int32MultiArray()
                cone_msg.data = self.zhuitong_relativ_position
                self.cone_publisher.publish(cone_msg)
            #这里设置一下状态
            if (self.zhuitong_relativ_position[0] != 0  and self.zhuitong_relativ_position[1] > self.zhuitongyuzhi):
                self.state = 1#避障导航模式
                # self.left_cone = int(self.zhuitong_relativ_position[0] - self.zhuitong_relativ_position[2]) - self.turn_error   #校正左转点
                # self.right_cone = int(self.zhuitong_relativ_position[0] + self.zhuitong_relativ_position[2]) + self.turn_error   #校正右转点
            else :
                self.state = 0 #普通导航模式

            if dt_1 > 1000:
                if self.qrcode_info == 0:
                    self.get_logger().info(f"开始识别二维码") #确保只打印一次
                    self.qrcode_info = 1
                #image_roi = cv_image[0:480,0:640] #截取图片，先高后宽 [50:450,0:640]
                # 选择解码方法，注释掉不需要的部分以使用另一种方法
                self.decode_qr_code_pyzbar(cv_image)  # 使用 pyzbar 解码

        if(self.task == 3):
            time = self.get_clock().now()
            dt = (time - self.task3_time).nanoseconds * 1e-9

            prediction_bbox,class_name = FCOS.fcos(self.model_path,cv_image)
            #为防止干扰，4秒内识别锥桶，不识别停车点；4秒后识别停车点，不识别锥桶
            if class_name == 'obstcle':
                self.zhuitong_relativ_position = prediction_bbox[:4]
                #把坐标发布出去方便调试
                cone_msg = Int32MultiArray()
                cone_msg.data = self.zhuitong_relativ_position
                self.cone_publisher.publish(cone_msg)

            if class_name == 'p':
                #获取P点的中点与宽高(x,y,w,h)
                self.park_relativ_position = prediction_bbox[:4]
                #把坐标发布出去方便调试
                park_msg = Int32MultiArray()
                park_msg.data = self.park_relativ_position
                self.park_publisher.publish(park_msg)

            #这里设置一下状态
            if (self.zhuitong_relativ_position[0] != 0  and self.zhuitong_relativ_position[1] > self.zhuitongyuzhi):
                self.state = 1
                # self.left_cone = int(self.zhuitong_relativ_position[0] - self.zhuitong_relativ_position[2]) - self.turn_error   #校正左转点
                # self.right_cone = int(self.zhuitong_relativ_position[0] + self.zhuitong_relativ_position[2]) + self.turn_error   #校正右转点
            elif(self.park_relativ_position[0] != 0):
                self.state = 3 
            else:
                self.state = 0
           
    def sign4return_callback(self,msg): #sign4return话题的回调函数，在foxglove上显示

    # 创建 Sign 消息实例
        if msg.data == 6:
            self.flag = 1
            self.state = 0
            self.task = 3
            self.task3_info = 1

            # 定位任务三中途目标点
            self.target_position[0] = 0  # 目标x和y坐标
            self.target_position[1] = 0
            self.target_angular =  0   
            self.get_logger().info(f"任务三")
            self.get_logger().info(f"结束遥感")

            self.task3_time = self.get_clock().now()

            self.position[0] = 2.5
            self.position[1] = 2.2
            self.angular = -np.pi/2

            ##这里会重置里程计到(2,2)

        if msg.data == -1:
            self.flag = 1
            self.state = 0
            self.task = 1
            #前往二维码区
            self.target_position[0] = 4.6  # 目标x和y坐标
            self.target_position[1] = 1.8
            
            self.target_angular =  0             
            self.get_logger().info(f"任务一")
            self.get_logger().info(f"重置里程计")

            self.task1_time = self.get_clock().now()

            self.angular = 0
            self.position[0] = 0.68
            self.position[1] = 0.2
            ##这里重置了里程计，将他的位置初始化成（0.5，0.2）
            
        if msg.data == 5:
            self.flag = 0
            self.task = 2
            self.get_logger().info(f"任务二")
            self.get_logger().info(f"C区进行遥测")

        return
        
    def imu_callback(self, msg):  # 回调函数，接收IMU数据，并调用navigation_control进行任务一，任务三的控制

        angular_velocity = np.array([msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z])  
        
        if len(self.angular_velocity_sample) <= self.sample_size:
            self.angular_velocity_sample.append(angular_velocity[2])   
        if len(self.angular_velocity_sample) == self.sample_size:
                # 计算偏移量
            self.bias_correction = np.mean(self.angular_velocity_sample)
            self.get_logger().info('偏移量: {}'.format(self.bias_correction))

        if  len(self.angular_velocity_sample) > self.sample_size: 
             self.imu_z = angular_velocity[2]-self.bias_correction

        '''
            current_time = self.get_clock().now()
            dt = (current_time - self.imu_last_time).nanoseconds * 1e-9  # 转换为秒
            self.imu_last_time = current_time

            self.angular += (angular_velocity[2]-self.bias_correction) * dt  # 更新角度
            #限制角度范围在[-pi, pi]之间
            if self.angular > np.pi:
                self.angular = self.angular - 2*np.pi
            if self.angular < -np.pi:
                self.angular = 2*np.pi + self.angular    
        '''

        if self.task == 1 :
            self.navigation_control()
        if self.task == 3 :
            distance = math.sqrt(( self.target_position[1] - self.position[1] )**2 + ( self.target_position[0] - self.position[0] )**2)
            #这里小车常常进不去if，因为小车速度快，拐角小，线速度角速度耦合，所以速度大的时候总是在目标点周围却没有满足if条件，就会转圈圈
            #适当调节self.stop_distance与self.vx
            if distance <= self.stop_distance:
                self.target_position[0] = -0.1  # 这里小车位置不太准，不一定设定成0.0,0.0，可以设置成参数具体调整即可
                self.target_position[1] = -0.1
            self.navigation_control()
            if self.task3_info == 1:
                self.get_logger().info('任务三目标位置位置: x={:.6f}, y={:.6f},state={}'.format(self.target_position[0], self.target_position[1],self.state))
                self.task3_info = 0
            
    def robotvel_callback(self , msg):  #回调函数，接收小车速度，并更新小车位置，角度(当前位置: x={:.6f}, y={:.6f}, 角度={:.6f},状态{},任务{})
        x_speed = msg.x
        y_speed = msg.y
        z_speed = msg.z
        current_time = self.get_clock().now()
        dt = (current_time - self.imu_last_time).nanoseconds * 1e-9  # 转换为秒
        self.imu_last_time = current_time

        
        self.position[0]+=1.13*( x_speed * math.cos(self.angular) ) * dt
        self.position[1]+=1.03*( x_speed * math.sin(self.angular) ) * dt
        

        if (x_speed != 0):
            self.angular = self.imu_z * dt + self.angular
            #限制角度范围在[-pi, pi]之间
            if self.angular > np.pi:
                self.angular = self.angular - 2*np.pi
            if self.angular < -np.pi:
                self.angular = 2*np.pi + self.angular    

        # 计算时间差
        elapsed_time = (current_time - self.last_log_time_vel_callback).nanoseconds * 1e-9  # 转换为秒
        if elapsed_time >= 3.0:  # 控制日志 3 秒输出一次
            self.get_logger().info(
                '当前位置: x={:.6f}, y={:.6f}, 角度={:.6f}, 状态{}, 任务{}'.format(
                    self.position[0], self.position[1], self.angular, self.state, self.task
                )
            )
            self.last_log_time_vel_callback = current_time  # 更新日志时间
        #self.get_logger().info('当前位置: x={:.6f}, y={:.6f}, 角度={:.6f},状态{},任务{}'.format(self.position[0], self.position[1], self.angular,self.state,self.task))

    def navigation_control(self): #总导航控制逻辑
        erro = math.sqrt(( self.target_position[1] - self.position[1] )**2 + ( self.target_position[0] - self.position[0] )**2)
        if ( erro > self.stop_distance):
            if self.flag == 1:
                if self.state == 0 :
                    self.navigation_0(self.target_position[0],self.target_position[1])
                elif self.state == 1 :
                    self.navigation_1()
                elif self.state == 2 :
                    self.navigation_2()
                elif self.state == 3:
                    #self.navigation_0(self.target_position[0],self.target_position[1])
                    self.navigation_3()
            else:
                twist_msg = Twist()
                twist_msg.linear.x = 0.00  # 示例：将 x 坐标复位
                twist_msg.angular.z = 0.00  # 使用 角速度复位
                # 发布 Twist 消息
                self.cmd_pub.publish(twist_msg)     

        elif( erro < self.stop_distance):
            cmd_msg = Twist()  
            cmd_msg.linear.x = 0.0  # 设置线性速度  
            cmd_msg.angular.z = 0.0  # 设置角速度  
            self.cmd_pub.publish(cmd_msg)  # 发布控制指令
    
    def navigation_0(self,target_x,target_y): #state=0 无避障简单惯导
                    
        target_angle = np.arctan2((target_y - self.position[1]) , (target_x - self.position[0]))
        angular =  target_angle - self.angular  # 计算角的方向的插值  

        # 构造控制指令  
        cmd_msg = Twist()  
        '''
        if self.task == 3:
            cmd_msg.linear.x = self.vx_3  # 设置线性速度  
            cmd_msg.angular.z = 1 * angular  # 设置角速度  
        
        if self.task == 1:
        '''
        cmd_msg.linear.x = self.vx
        cmd_msg.angular.z = 2 * angular  # 设置角速度  
        self.cmd_pub.publish(cmd_msg)  # 发布控制指令  

        # 打印当前的角度和转速（调试用）  
        # 计算时间差
        current_time_navagation_0 = self.get_clock().now()
        elapsed_time = (current_time_navagation_0 - self.last_log_time_navagation_0).nanoseconds * 1e-9  # 转换为秒
        if elapsed_time >= 3.0:  # 控制日志 3 秒输出一次
            self.get_logger().info('目标角度: target_angle={:.6f},'.format(target_angle))
            self.get_logger().info('当前角速度: angular={:.6f},'.format(cmd_msg.angular.z))
            self.last_log_time_navagation_0 = current_time_navagation_0 
  
    def navigation_1(self): #state=1 避障惯导

        self.get_logger().info(f"识别到锥桶")

        #锥桶在右，向左避障
        if(self.zhuitong_relativ_position[0] > 320 and self.zhuitong_relativ_position[0] < 500 ):
            if(self.task == 1):
                    self.get_logger().info(f"任务1向左避障")
                    self.navigation_0(self.position[0]+0.2,self.position[1] + 2.0)
                    #time.sleep(0.05)
            elif(self.task == 3):
                    self.navigation_0(self.position[0]-0.2,self.position[1]-3.0)
                    #self.action_cone()
                    self.get_logger().info(f"任务3向左避障")
                    #time.sleep(0.05)
        #锥桶在左，向右避障
        elif(self.zhuitong_relativ_position[0] < 320 and self.zhuitong_relativ_position[0] >150 ):
            if(self.task == 1):
                     self.navigation_0(self.position[0]+0.2,self.position[1] - 2.0)
                     self.get_logger().info(f"任务1向右避障")
                     #time.sleep(0.05)
            elif(self.task == 3):
                    #self.action_cone()
                    self.navigation_0(self.position[0]-0.2,self.position[1]+ 5.0)   
                    # cmd_msg = Twist()  
                    # cmd_msg.linear.x = self.vx
                    # cmd_msg.angular.z = - 2.0
                    # self.cmd_pub.publish(cmd_msg)  # 发布控制指令  
                    self.get_logger().info(f"任务3向右避障")
                    #time.sleep(0.05)
        else:
            self.navigation_0(self.target_position[0],self.target_position[1])

    def navigation_2(self): #state=2，二维码导航控制 实际上并未使用
        cmd_msg = Twist()  
        cmd_msg.linear.x = self.vx  # 设置线性速度  
        cmd_msg.angular.z = - self.kp * (self.park_relativ_position[0] - 320) # 设置角速度
        self.get_logger().info('前往二维码角速度: angular.z={:.6f},'.format(cmd_msg.angular.z ))
        self.cmd_pub.publish(cmd_msg)  # 发布控制指令

    def navigation_3(self): #state=3，cv识别到停车点，进行导航，
        cmd_msg = Twist()  
        cmd_msg.linear.x = self.vx_3  # 设置线性速度  
        cmd_msg.angular.z = - self.kp * (self.park_relativ_position[0] - 320) # 设置角速度

        self.cmd_pub.publish(cmd_msg)  # 发布控制指令

        current_time_navagation_3 = self.get_clock().now()
        elapsed_time = (current_time_navagation_3 - self.last_log_time_navagation_3).nanoseconds * 1e-9  # 转换为秒
        if elapsed_time >= 3.0:  # 控制日志 3 秒输出一次
            self.get_logger().info('返回P点速度: linear.x={:.6f},'.format(cmd_msg.linear.x ))
            self.get_logger().info('返回P点角速度: angular.z={:.6f},'.format(cmd_msg.angular.z ))
            self.last_log_time_navagation_3 = current_time_navagation_3 

    def action_cone(self): #另一种避障逻辑，并未使用
        twist_msg = Twist()
        if self.zhuitong_relativ_position[0] >= 320:
            if (320-self.left_cone ) > 0:
                twist_msg.angular.z = self.turn_w * (320-self.left_cone)
                if self.task ==1:
                    twist_msg.linear.x = self.vx
                if self.task ==3:
                    twist_msg.linear.x = self.vx_3
                self.get_logger().info(f"{self.left_cone}")
                self.get_logger().info(f"{twist_msg.angular.z}")  
                self.cmd_pub.publish(twist_msg)    
                return                  
            else :
                self.navigation_0(self.target_position[0],self.target_position[1])
                
        if self.zhuitong_relativ_position[0] < 320:
            if (320-self.right_cone ) < 0:
                twist_msg.angular.z = self.turn_w * (320-self.right_cone)
                if self.task ==1:
                    twist_msg.linear.x = self.vx
                if self.task ==3:
                    twist_msg.linear.x = self.vx_3
                self.get_logger().info(f"{self.right_cone}")  
                self.get_logger().info(f"{twist_msg.angular.z}")  
                self.cmd_pub.publish(twist_msg) 
                return
            else :
                self.navigation_0(self.target_position[0],self.target_position[1]) 
    
    def perspective_point(self,x,y): #透视变换函数，输入yolo锥桶底边中点，输出相对小车实际位置，右正，左负
        homogeneous_point = np.array([x, y, 1.0], dtype=np.float32)
        transformed_point = np.dot(self.matrix, homogeneous_point)
        transformed_point /= transformed_point[2]
        #if int(round(transformed_point[0])) > 200 or int(round(transformed_point[1])) > 200 or int(round(transformed_point[0])) < 0 or int(round(transformed_point[1])) < 0:
        #    return None
        #else:
        location =(-1.0 * (0.01*(transformed_point[0] - 100.0)), 0.01*(200.0 - transformed_point[1]))
        return location
        #参数回调函数,回调使用       

    def decode_qr_code_pyzbar(self, image):  #识别到二维码结果后停车，进入任务二
        # 使用 pyzbar 进行二维码解码
        #gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        image = cv2.resize(image,(320,240)) #处理更快，但是精度更低
        decoded_objects = decode(image)
        
        # 遍历结果并判断是否是顺时针或逆时针二维码
        for obj in decoded_objects:
            qr_code_data = obj.data.decode('utf-8')
            if qr_code_data in ["ClockWise", "AntiClockWise"]:
                self.publish_sign_message(qr_code_data)
                self.flag  = 0 
                self.task = 2

                #停车
                twist_msg = Twist()
                twist_msg.linear.x = 0.00  # 示例：将 x 坐标复位
                twist_msg.angular.z = 0.00  # 使用 角速度复位
                self.cmd_pub.publish(twist_msg)
                self.get_logger().info(f"已复位")

                return  # 成功识别并发布后立即返回，避免重复处理
    
    def publish_sign_message(self, qr_code_data): #发布二维码的信息
        # 创建并发布 Sign 消息
        sign_msg = Sign()
        sign_msg.sign_data = 3 if qr_code_data == "ClockWise" else 4
        self.sign_switch_pub.publish(sign_msg)
        qrcode_msg = String()
        qrcode_msg.data =  f"二维码的识别结果为: {qr_code_data}"
        self.qrcode_publisher.publish(qrcode_msg)
        self.get_logger().info(f"已发布二维码信息: {qr_code_data}")

    def process_frame(self,frame):  #返回锥桶的x0,y0,w0,h0，底边中点与宽高
        x0 = 0
        y0 = 0
        w0 = 0
        h0 = 0
        im0 = frame[self.up:450, self.left:600]
        hsv_img = cv2.cvtColor(im0, cv2.COLOR_BGR2HSV)

        # Define range of target color in HSV
        lower_hsv1 = np.array([0, 0, 0])
        lower_hsv2 = np.array([82, 255, 255])
        upper_hsv1 = np.array([0, 0, 0])
        upper_hsv2 = np.array([255, 90, 255])

        # Threshold the HSV image
        imgThreshLow = cv2.inRange(hsv_img, lower_hsv1, lower_hsv2)
        imgThreshHigh = cv2.inRange(hsv_img, upper_hsv1, upper_hsv2)
        threshed_img = cv2.bitwise_or(imgThreshLow, imgThreshHigh)

        # Find contours in the black regions of smoothed_img
        contours, hierarchy = cv2.findContours(255 - threshed_img, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)


        # Initialize variables to store the largest contour
        max_area = 0
        largest_rect = None
        largest_cnt = None

        # Identify the largest contour and store its bounding rectangle
        for cnt in contours:
            area = cv2.contourArea(cnt)
            if area > self.area:  # Filter small contours if needed
                x, y, w, h = cv2.boundingRect(cnt)
                if area > max_area:
                    max_area = area
                    largest_rect = (x, y, w, h)
                    largest_cnt = cnt

        # Draw bounding rectangles and centroids
        for cnt in contours:
            area = cv2.contourArea(cnt)
            if area > self.area:
                x, y, w, h = cv2.boundingRect(cnt)

                # Draw the largest rectangle in red, others in green
                if (x, y, w, h) == largest_rect:
                    color = (0, 0, 255)  # Red for the largest rectangle
                    x0 = int(x + self.left + w / 2)
                    y0 = int(y + self.up + h )
                    w0 = int(w)
                    h0 = int(h)         
     
        return x0,y0,w0,h0
    
    def park_frame(self,frame): #返回P点的x0,y0,w0,h0 中心点与宽，高
            x0 = 0
            y0 = 0
            w0 = 0
            h0 = 0
            im0 = frame[self.up_park:450, 0:self.right_park]#增大右边界就要减小lower_hsv2第二个阈值
            hsv_img = cv2.cvtColor(im0, cv2.COLOR_BGR2HSV)

            # Define range of target color in HSV
            lower_hsv1 = np.array([0, 0, 0])
            lower_hsv2 = np.array([47, 73, 255])
            upper_hsv1 = np.array([0, 0, 0])
            upper_hsv2 = np.array([255, 50, 255])#60

            # Threshold the HSV image，阈值内为白色（255）
            imgThreshLow = cv2.inRange(hsv_img, lower_hsv1, lower_hsv2)
            imgThreshHigh = cv2.inRange(hsv_img, upper_hsv1, upper_hsv2)
            threshed_img = cv2.bitwise_and(imgThreshLow, 255-imgThreshHigh)

            # Find contours in the black regions of smoothed_img
            contours, hierarchy = cv2.findContours(threshed_img, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

            # Initialize variables to store the largest contour
            max_area = 0
            largest_rect = None
            largest_cnt = None

            # Identify the largest contour and store its bounding rectangle
            for cnt in contours:
                area = cv2.contourArea(cnt)
                if area > self.area:  # Filter small contours if needed
                    x, y, w, h = cv2.boundingRect(cnt)
                    if area > max_area:
                        max_area = area
                        largest_rect = (x, y, w, h)
                        largest_cnt = cnt

            # Draw bounding rectangles and centroids
            for cnt in contours:
                area = cv2.contourArea(cnt)
                if area > self.area:
                    x, y, w, h = cv2.boundingRect(cnt)

                    # Draw the largest rectangle in red, others in green
                    if (x, y, w, h) == largest_rect:

                        x0 = int(x + w / 2)
                        y0 = int(y + self.up_park + h / 2)
                        w0 = int(w)
                        h0 = int(h)
                
            return  x0,y0,w0,h0 
    
    def parameters_callback(self,paramters): #参数设置回调函数
        for paramter in paramters:
            self.get_logger().info("Parameter: {} - Value: {}".format(paramter.name, paramter.value))
            if paramter.name == "kp":
                self.kp = paramter.value
            elif paramter.name == "vx":
                self.vx = paramter.value   
            elif paramter.name == "vx_3":
                self.vx_3 = paramter.value   
            elif paramter.name == "stop_distance":
                self.stop_distance = paramter.value  
            elif paramter.name == "safe_distance":
                self.safe_distance = paramter.value
            elif paramter.name == "turn_time_1":
                self.turn_time_1 = paramter.value  
            elif paramter.name == "turn_time_2":
                self.turn_time_2 = paramter.value
            elif paramter.name == "turn_v":
                self.turn_v = paramter.value  
            elif paramter.name == "turn_w":
                self.turn_w = paramter.value 
            elif paramter.name == "turn_error":
                self.error = paramter.value    
            elif paramter.name == "zhuitongyuzhi":
                self.zhuitongyuzhi = paramter.value  
            elif paramter.name == "radius_cone":
                self.radius_cone = paramter.value  
     
        return SetParametersResult(successful=True)

def main(args=None):  
    rclpy.init(args=args)  # 初始化rclpy  
    navigation = InertialNavigationNode()  #
    rclpy.spin(navigation)
    navigation.destroy_node()  # 销毁节点
    rclpy.shutdown()
  
if __name__ == '__main__':  
    main()  # 入口函数

